#include "Control.h"


/*
 * 此模块用于初始化各个模块
 *
 */
void Move_Init(void)
{
    //BLDC初始化

    BLDC_HallInit();
    BLDCInit();
    CCU6_InitConfig_u(CCU60, CCU6_Channel1, 20);  // 20us进入一次中断

    //蓝牙初始化
    UART_InitConfig(UART0_RX_P14_1, UART0_TX_P14_0, 115200);    //蓝牙模块接口初始化

    //舵机初始化
    ServoInit();
    ServoCtrl(ServoALL, 850);

    //编码器初始化
    EncInit();

    //摄像头初始化
    CAMERA_Init(30);

}

/*
 * 此函数为科目一的白线检测模块
 *
 *
*/
boolean Camera_DetectWhiteLine(void) {
    static uint64 startTime = 0;
    // 获取二值化图像
    Get_Bin_Image(3);
    
    // 首次调用时记录启动时间
    if(startTime == 0) {
        startTime = IfxStm_get(&MODULE_STM0) / 1000;
        return FALSE;
    }

    // 启动后前2秒不检测白线
    if((IfxStm_get(&MODULE_STM0) / 1000 - startTime) < 2000) {
        return FALSE;
    }

    // 检测底部中心区域的白线
    int whiteCount = 0;
    for(int i = LCDH-10; i < LCDH; i++) {
        for(int j = LCDW/2-15; j < LCDW/2+15; j++) {
            if(Pixle[i][j] > 180) { // 白线阈值
                whiteCount++;
            }
        }
    }

    
    // 如果检测到足够多的白点，则认为检测到白线
    return (whiteCount > 50);
}

/*
 * 偏航角计算函数
 *
 *
*/
// 修改CalculateYaw函数增加数据检查
float CalculateYaw(short gyrox, short gyroy, short gyroz) {
    static float yaw = 0.0f;
    static uint64 lastTime = 0;
    
    // 检查陀螺仪数据是否在合理范围内
    if(abs(gyroz) > 32700) { // 异常值检查
        return yaw; // 返回上次有效值
    }
    
    // 获取当前时间(ms)
    uint64 currentTime = IfxStm_get(&MODULE_STM0) / 1000;
    
    // 计算时间差(秒)
    float dt = (currentTime - lastTime) / 1000.0f;
    lastTime = currentTime;
    
    // 积分计算偏航角(使用Z轴角速度)
    yaw += gyroz * dt * 0.001065f; // 0.001065为陀螺仪灵敏度系数
    
    // 角度归一化到0-360度
    if(yaw > 360.0f) yaw -= 360.0f;
    if(yaw < 0.0f) yaw += 360.0f;
    
    return yaw;
}


/*
 * 此模块为蓝牙控制运动，链接蓝牙后即可使用
 *
 */


void Motion_Control(void)
{
    char txt[32];   //存储显示字符
    uint8 bit_Con;
    signed short duty = 850;
    GPIO_LED_Init();
    GPIO_KEY_Init();
    Display_Init();
    Display_CLS(U16_WHITE);
    Move_Init();

    while(1)
    {
        //蓝牙参数读取
        bit_Con = 'x';
        for(int i = 0; i < UART_GetCount(UART0); i++)
        {
            bit_Con = UART_GetChar(UART0);
        }
/***************************************************************************************************************************/
        //电机控制
        if (KEY_Read(KEY0) == 0 || bit_Con ==  'K') // 按下KEY0键，占空比减小，或者蓝牙控制中的down键
        {
            //LED_Ctrl(LED0,ON);
            //sprintf(txt, "key0 down ");
            if (BLDC_duty > -ATOM_PWM_MAX)
                BLDC_duty -= 50;
            //sprintf(txt, "HALL:%05d  BLDC:%05d  ",Get_Hall_State(),BLDC_duty);
            while(KEY_Read(KEY0) == 0);

        }
        if (KEY_Read(KEY2) == 0 || bit_Con == 'G') // 按下KEY2键，占空比加大，或者蓝牙控制中的up键
        {
            if (BLDC_duty < ATOM_PWM_MAX) // 满占空比为12500
                BLDC_duty += 50;
            while(KEY_Read(KEY2) == 0);
        }
        if (KEY_Read(KEY1) == 0 || bit_Con == 'I') // 按下KEY1键，占空比中值
        {
            BLDC_duty = 500;
        }

        if(bit_Con == 'A')
        {
            char a[20];
            //停止电机函数
            LQ_BLDCStop();
            Display_CLS(U16_RED);
            sprintf(a,"STOP!");
            Display_showString(0,0,&a,U16_WHITE,U16_BLUE,16);
            //如果1.5秒后编码器传回值不为0，则强行卡死程序
            Delay_Ms(1500);
            if(ENC_GetCounter(ENC1_InPut_P33_7) != 0 || ENC_GetCounter(ENC2_InPut_P02_8) != 0 || ENC_GetCounter(ENC3_InPut_P10_3) || ENC_GetCounter(ENC4_InPut_P20_3))
            {
                Bluetooth_Char();
            }
        }
        sprintf(txt, "HALL:%05d  BLDC:%05d  ",Get_Hall_State(),BLDC_duty);
        Display_showString(0, 3, txt, U16_RED, U16_BLUE, 16); // 将txt中 内容显示出来
        LQ_BLDCCtrl();

/*************************************************************************************************************************/
        //舵机控制
        if (!KEY_Read(KEY0) || bit_Con == 'J')
        {
            if (duty > 10) // 防止duty超
            {
                duty -= 10;
            }
        }
        if (!KEY_Read(KEY1) || bit_Con == 'H')
        {
            duty += 10;
        }
        if (!KEY_Read(KEY2) || bit_Con == 'F')
        {
            duty = Servo_Center_Mid;
        }
        ServoCtrl(ServoALL, duty); // 中值
        sprintf(txt, "Servo duty:%04d ", duty);
        Display_showString(1, 2, txt, U16_BLACK, U16_GREEN, 16); // 显示出库实际脉冲数，以便灵活调整
    }
}



// 往返跑测试函数
void RoundTripTest(float targetSpeed) {
    static pid_param_t speed_pid;
    static pid_param_t yaw_pid;
    char txt[32];
    short aacx, aacy, aacz, gyrox, gyroy, gyroz;
    short magx, magy, magz;
    int encoderCount = 0;
    boolean reachedEnd = FALSE;

    // 初始化PID参数
    speed_pid.kp = 0.5f;
    speed_pid.ki = 0.01f;
    speed_pid.kd = 0.1f;
    speed_pid.imax = 1000;
    yaw_pid.kp = 10.0f; 
    yaw_pid.ki = 0.1f;
    yaw_pid.kd = 5.0f;
    yaw_pid.imax = 100;
    
    // 初始化显示
    Display_CLS(U16_WHITE);
    Display_showString(0, 0, "Round Trip Test", U16_BLACK, U16_WHITE, 16);
    sprintf(txt, "Speed: %.1f", targetSpeed);
    Display_showString(0, 1, txt, U16_BLACK, U16_WHITE, 16);
    
    // 主循环
    while(1) {
        // 1. 读取传感器数据
        ICM_Get_Raw_data(&aacx, &aacy, &aacz, &gyrox, &gyroy, &gyroz);
        Gyro_Get_Mag_data(&magx, &magy, &magz);
        encoderCount = ENC_GetCounter(ENC1_InPut_P33_7);
        
        // 2. 摄像头检测白线
        boolean lineDetected = Camera_DetectWhiteLine();
        
        // 3. 控制逻辑
        if(!reachedEnd) {
            // 前进阶段
            if(lineDetected) {
                reachedEnd = TRUE;
                BLDC_duty = 0; // 准备掉头
                ServoCtrl(ServoALL, 1000); // 转向
                
                // 基于陀螺仪数据的掉头检测
                float initialYaw = CalculateYaw(gyrox, gyroy, gyroz);
                float targetYaw = initialYaw + 180.0f; // 目标偏航角(180度掉头)
                if(targetYaw > 360.0f) targetYaw -= 360.0f;
                
                while(1) {
                    ICM_Get_Raw_data(&aacx, &aacy, &aacz, &gyrox, &gyroy, &gyroz);
                    float currentYaw = CalculateYaw(gyrox, gyroy, gyroz);
                    
                    // 检查是否完成掉头(±10度容差)
                    if(fabs(currentYaw - targetYaw) < 10.0f || 
                       fabs(currentYaw - targetYaw + 360.0f) < 10.0f) {
                        break;
                    }
                    
                    Delay_Ms(10); // 控制周期
                }
            } else {
                // 速度控制
                BLDC_duty = (int)(targetSpeed * 100);
                
                // 陀螺仪角度矫正
                float yaw_error = 0 - CalculateYaw(gyrox, gyroy, gyroz); // 目标偏航角为0
                float steer_adjust = PidLocCtrl(&yaw_pid, yaw_error);
                ServoCtrl(ServoALL, 850 + (int)steer_adjust);
            }
        } else if(lineDetected){
            // 返回阶段
            if(lineDetected) {
                // 到达起点，停止
                LQ_BLDCStop();
                Display_showString(0, 2, "Test Completed!", U16_GREEN, U16_WHITE, 16);
                break;
            } else {
                // 保持返回速度
                BLDC_duty = (int)(targetSpeed * 100);
                
                // 陀螺仪角度矫正
                float yaw_error = 0 - CalculateYaw(gyrox, gyroy, gyroz); // 目标偏航角为0
                float steer_adjust = PidLocCtrl(&yaw_pid, yaw_error);
                ServoCtrl(ServoALL, 850 + (int)steer_adjust);
            }
        }
        
        // 4. 电机控制和数据显示
        LQ_BLDCCtrl();
        sprintf(txt, "Encoder: %d", encoderCount);
        Display_showString(0, 3, txt, U16_BLACK, U16_WHITE, 16);
        
        Delay_Ms(10); // 控制周期
    }
}


void Subject1(void) {
    Move_Init();
    RoundTripTest(25.0f); // 25.0为速度参数，可调整
    return;
}

void Subject2(void)
{
    Display_CLS(U16_YELLOW);
    Delay_Ms(500);
    return;
}

void Subject3(void)
{
    char txt[32];   //存储显示字符
    uint8 bit_S1;
    signed short duty = 750;
    GPIO_LED_Init();
    GPIO_KEY_Init();
    Display_Init();
    Display_CLS(U16_WHITE);
    Move_Init();
    while(1)
    {
        bit_S1 = 'x';
        for(int i = 0; i < UART_GetCount(UART0); i++)
        {
            bit_S1 = UART_GetChar(UART0);
        }
        if(bit_S1 == 'B')
        {
            while(1)
            {
                BLDC_duty=0;
                if(BLDC_duty < 2500)
                {
                    while(1)
                    {
                        BLDC_duty += 50;
                        if(BLDC_duty == 2500)
                        {
                            break;
                        }
                        Delay_Ms(100);
                        LQ_BLDCCtrl();
                    }
                    sprintf(txt,"BLDC_duty = %d",BLDC_duty);
                    Display_showString(0, 3, txt, U16_BLACK, U16_GREEN, 16);
                }
                if(BLDC_duty == 2500)
                {
                    break;
                }
            }
            Delay_Ms(5500);
            while(1)
            {
                duty = 1000;
                ServoCtrl(ServoALL, duty);
                if(duty == 1000)
                {
                    break;
                }
            }
            Delay_Ms(1800);
            duty = 750;
            ServoCtrl(ServoALL, duty);
            while(1)
            {
                if(BLDC_duty < 2500)
                {
                    BLDC_duty += 50;
                }
                Delay_Ms(100);
                LQ_BLDCCtrl();
                sprintf(txt,"BLDC_duty = %d",BLDC_duty);
                Display_showString(1, 2, txt, U16_BLACK, U16_GREEN, 16);
                if(BLDC_duty == 2500)
                {
                    break;
                }
            }
            Display_CLS(U16_GREEN);
            Delay_Ms(5000);
            while(1)
            {
                if(BLDC_duty > 500)
                {
                    BLDC_duty -= 50;
                    Delay_Ms(100);
                }
                LQ_BLDCCtrl();
                if(BLDC_duty == 500)
                {
                    break;
                }
            }
            Display_CLS(U16_WHITE);
            LQ_BLDCStop();

        }
    }
    if(bit_S1 == 'A')
    {
        char a[20];
        //停止电机函数
        LQ_BLDCStop();
        Display_CLS(U16_RED);
        sprintf(a,"STOP!");
        Display_showString(0,0,&a,U16_WHITE,U16_BLUE,16);
        //如果1.5秒后编码器传回值不为0，则强行卡死程序
        Delay_Ms(1500);
        if(ENC_GetCounter(ENC1_InPut_P33_7) != 0 || ENC_GetCounter(ENC2_InPut_P02_8) != 0 || ENC_GetCounter(ENC3_InPut_P10_3) || ENC_GetCounter(ENC4_InPut_P20_3))
        {
            Bluetooth_Char();
        }
    }
}


/*
 * 以下部分是延时转弯，仅用于测试车辆极限性能
 * 不可用于比赛代码！！！
 * 不可用于比赛代码！！！
 *
 */
/*{
    char txt[32];   //存储显示字符
    uint8 bit_S1;
    signed short duty = 750;
    GPIO_LED_Init();
    GPIO_KEY_Init();
    Display_Init();
    Display_CLS(U16_WHITE);
    Move_Init();
    while(1)
    {
        bit_S1 = 'x';
        for(int i = 0; i < UART_GetCount(UART0); i++)
        {
            bit_S1 = UART_GetChar(UART0);
        }
        if(bit_S1 == 'B')
        {
            while(1)
            {
                BLDC_duty=0;
                if(BLDC_duty < 2500)
                {
                    while(1)
                    {
                        BLDC_duty += 50;
                        if(BLDC_duty == 2500)
                        {
                            break;
                        }
                        Delay_Ms(100);
                        LQ_BLDCCtrl();
                    }
                    sprintf(txt,"BLDC_duty = %d",BLDC_duty);
                    Display_showString(0, 3, txt, U16_BLACK, U16_GREEN, 16);
                }
                if(BLDC_duty == 2500)
                {
                    break;
                }
            }
            Delay_Ms(5500);
            while(1)
            {
                duty = 1000;
                ServoCtrl(ServoALL, duty);
                if(duty == 1000)
                {
                    break;
                }
            }
            Delay_Ms(1650);
            duty = 850;
            ServoCtrl(ServoALL, duty);
            while(1)
            {
                if(BLDC_duty < 2500)
                {
                    BLDC_duty += 50;
                }
                Delay_Ms(100);
                LQ_BLDCCtrl();
                sprintf(txt,"BLDC_duty = %d",BLDC_duty);
                Display_showString(1, 2, txt, U16_BLACK, U16_GREEN, 16);
                if(BLDC_duty == 2500)
                {
                    break;
                }
            }
            Display_CLS(U16_GREEN);
            Delay_Ms(5000);
            while(1)
            {
                if(BLDC_duty > 500)
                {
                    BLDC_duty -= 50;
                    Delay_Ms(100);
                }
                LQ_BLDCCtrl();
                if(BLDC_duty == 500)
                {
                    break;
                }
            }
            Display_CLS(U16_WHITE);
            LQ_BLDCStop();

        }
    }
    if(bit_S1 == 'A')
    {
        char a[20];
        //停止电机函数
        LQ_BLDCStop();
        Display_CLS(U16_RED);
        sprintf(a,"STOP!");
        Display_showString(0,0,&a,U16_WHITE,U16_BLUE,16);
        //如果1.5秒后编码器传回值不为0，则强行卡死程序
        Delay_Ms(1500);
        if(ENC_GetCounter(ENC1_InPut_P33_7) != 0 || ENC_GetCounter(ENC2_InPut_P02_8) != 0 || ENC_GetCounter(ENC3_InPut_P10_3) || ENC_GetCounter(ENC4_InPut_P20_3))
        {
            Bluetooth_Char();
        }
    }
}*/



